THE CATHOLIC UNIVERSITY OF AMERICA 
DEPARTMENT OF ELECTRICAL ENGINEERING 


TRAJECTORY PLANNING AND CONTROL 
OF A 6 DOF MANIPULATOR WITH 
STEWART PLATFORM-BASED MECHANISM 

Charles C. Nguyen 

Principal Investigator and Associate Professor 

and 

Sami Antrazi 

Graduate Research Assistant 


submitted to 

Dr. Charles E. Campbell, Jr. 
Code 714.1 

Goddard Space Flight Center (NASA) 
Greenbelt, Maryland 


July 1990 



SUMMARY OF THE REPORT 


This report presents the research results obtained from the research grant entitled 
“Control of Robot Manipulator Compliance,” funded by the Goddard Space Flight Center 
(NASA) under a research grant with Grant Number NAG 5-780, for the period between 
February 1st , 1990 and July 8l } 1990. 

This report deals with the trajectory planning and control of a robot manipulator 
that has 6 degrees of freedom and was designed based on the mechanism of the Stew- 
art Platform. First the main components of the manipulator will be described and its 
operation will be explained. We then briefly present the solutions for the forward and 
inverse kinematics of the manipulator. After that, two trajectory planning schemes will 
be developed using the manipulator inverse kinematics to track straight lines and cir- 
cular paths. Finally experiments conducted to study the performance of the developed 
planning schemes in tracking a straight line and a circle will be presented and discussed. 



1 Introduction 

Successful robotic assembly of parts requires that the robot end-effector be able to 
perform very precise motion. As a result, research effort has been enormously spent 
[1,12] to study the application of closed-kinematic chain (CKC) mechanism in the 
design of robot manipulators performing high precision motion because CKC mecha- 
nism generally provides better positioning capability than open-kinematic chain (OKC) 
mechanism. CKC mechanism was first implemented in the design of the Stewart Plat- 
form [1] originally intended for simulating aircraft motion. Later, the Stewart platform 
attracted considerable interest of robotic researchers and was proposed in the design 
of several robot manipulators and end-effectors [2,8]. Hunt [2,3] developed numerous 
structural designs for robot manipulators using the mechanism of the Stewart Platform 
which initiated intensive research in [4,5]. Sugimoto and Duffy [6] employed the theory 
of linear algebra and screw systems to obtain a general method which describes the 
instantaneous link motion of a single closed-loop mechanism. Yang and Lee [7] stu le 
the feasibility of manipulators designed based on the Stewart Platform whose inverse 
dynamics and kinematics were later investigated by Do and Yang [8] and Sugimoto [9J. 
Nguyen and Pooran applied the Lagrangian formulation to derive dynamical equations 
for a six-degree-of-freedom (DOF) CKC manipulator [10] whose kinematics was also 
developed in [11]. Later, they derived a learning-based control scheme for the above 

manipulator to perform repetitive tasks [12]. 

A Stewart platform-based manipulator was designed and built at the Goddard Space 
Flight Center (GSFC) [13] to serve as a testbed for evaluating the feasibility of au- 
tonomous assembly of parts in space. In this report, we first describe the main compo- 
nents of the manipulator and explain its operation. We then briefly review the forward 
and inverse kinematics of the manipulator. After that, a Cartesian trajectory planning 
scheme is developed and employed with a trajectory control scheme using the manip- 
ulator inverse kinematics. Finally experiments conducted to study the performance o 
the developed trajectory planning and control scheme are presented and discussed. 


2 The 6 DOF Robot Manipulator 

This section is devoted to briefly describe the main components of the manipulator and 
present the solutions for its inverse and forward kinematics. 

2.1 Description of Hardware 

Figures 1 and 2 show the robot manipulator which was designed based on the Stewart 
Platform mechanism and mainly consists of a lower base platform, an upper payloa 
platform, a compliant platform, a gripper and six linear actuators. The movable payloa 
platform is supported above the stationary base platform by six axially extensible rods 
with ballnuts and ballscrews providing the extensibility. Stepper motors were selected 
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to drive the ballscrews to extend or shorten the actuator lengths whose variations will 
in turn produce the motion of the payload platform and consequently the motion of a 
gripper attached to the compliant platform. Each end of the actuator links is mounted 
to the platforms by 2 rotary joints whose axes intersect and are perpendicular to each 
other. Passive compliance is provided through the compliant platform, which is sus- 
pended from the payload platform by six spring-loaded pistons arranged in the Stewart 
Platform mechanism. The compliance is passively provided by permitting strain on two 
opposing springs acting in the pistons. Thus the pistons are compressed and extended 
when resistive and gravitational forces are applied on the gripper. The rotation of each 
stepper motor is controlled by sending out proper commands to an indexer which then 
transmits proper pulse sequences to the stepper motor drive. Therefore, precise gripper 
motion can be produced by properly controlling the motions of six manipulator legs^ 
Figure 2 presents the planning and control scheme employed to control the motion of 
the manipulator gripper. A Cartesian path specified with desired starting and ending 
velocities and accelerations is converted into 6 Cartesian trajectories using a Carte- 
sian trajectory planning scheme. Then based upon the desired Cartesian trajectories, 
joint-space trajectories will be determined by a planner which sends proper commands 
through the RS232 port of a personal computer to the indexers. The indexer will then 
transmit pulses to the stepper motor drives where microstepping permits each revolution 
(360°) of the stepper motor to be equivalent to 25,000 steps. Therefore the drive rotates 
the stepper motor one angular increment of ^5 = 0.0144°, each time it receives one 
step pulse. Furthermore, through the linear motion converter system consisting of the 
ballnut and the ballscrew, each angular increment (=lstep) is converted into 8 ^-inches 
of linear translation of the manipulator leg. 


2.2 Solution of the Inverse Kinematics 

The lengths of the manipulator legs are selected as joint variables. To define the Carte- 
sian variables we proceed to assign coordinate frame {A} to the movable payload plat- 
form and {B} to the base platform as shown in Figure 2. The position and orientation 
of Frame {A} with respect to Frame {B} axe selected as the Cartesian variables in 
the sense that the position of Frame {A} is the position of its origin with respect to 
Frame {B}. Now denoting the angle between AA t and x A by A,, and the angle between 
BBi and x B by A, for i=l,2,. . . ,6, we have obtain A; = 60(z - 1)°; A, = 60(z - 1)° 
for i=l,3,5 and A, = + 9 B \ \ = *i-i + °a &r 1 =2,4,6. Furthermore, if Vector 

^a, = (a, x a iy a iz ) T describes the position of the attachment point A, with respect to 
Frame {A}, and Vector B b ,• = (b ix b iy b tz ) T the position of the attachment point B, with 
respect to Frame {B}, then they can be written as ^a, = [ r A cos(\i ) r A sm(\i ) 0 ] 
and B b , = [ r B cos{ A,) r B sin( A<) 0 ] r for i=l,2,. . . ,6 where represents the radius 

of the payload platform and r B that of the base platform. 

We proceed to consider the vector diagram for an ith actuator given in Figure 4. 
The length vector B q, = (q ix q iy ft,) T , expressed with respect to Frame {B} can be 
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computed by 


B q, = B a, - *b, (1) 

where Vector B a, and Vector B b, describe the positions of A, and respectively both 
in terms of Frame {B}. However, B a, can be computed by 

B a, = b R A &i + B d (2) 

where B R is the matrix representing the orientation of Frame {A} with respect to Frame 
{B} and Vector B d contains the Cartesian coordinates x, y, z of the origin, A of Frame 
{A} with respect to Frame {B} such that B d = [ x y z ] • 

Now substituting (2) into (1) and using U = \jq 2 x + q 2 y + we obtain after intensive 
simplifications: 

li 2 = x 2 + y 2 + z 2 + r\ + r| + 2(r n a il + r 12 a iy )(x - b ix ) 

+2(^21 a, x + r 22 u.j/)(y - b iy ) + 2(r 31 a ix + r 32 a iy )z - 2 (xb ix ), (3) 

for i=l,2,. . . ,6 where r tJ for ij=l,2,3 are the elements of the matrix B R. 

Equation (3) represents the solution to the inverse kinematic problem in the sense 
that for a given Cartesian configuration, composed of the position and orientation speci- 
fied by B b, and B R, respectively, the actuator lengths h for i=l,2,. . . ,6, can be computed 

using (3). 

2.3 Solution of the Forward Kinematics 

The forward kinematics concerns with the determination of the Cartesian position and 
orientation of the payload platform when the actuator lengths l, for i=l,2,. .., 6 , are 
given. Consequently, the forward kinematic problem can be formulated as to find x, y, z 
(position) and r tj (orientation) for i=j=l,2,3 to satisfy Equation (3) for a given set of /, 
for i=l,2,. . . ,6. Thus the problem is reduced to solving 6 highly nonlinear simultaneous 
equations with 6 unknowns, 3 of which represent the orientation. A closed-form solution 
does not generally exist, which leads us to seek an iterative numerical method to solve 
the above set of nonlinear equations. One widely used technique for solving nonlinear 
equations is the Newton-Raphson method, which is employed in this report to solve the 
forward kinematic problem. 


3 The Trajectory Planning Schemes 

Two trajectory planning schemes developed to control the motion of the robot manip- 
ulator gripper are presented in this section. The first scheme is designed for tracking 
straight lines while the second for arbitrary paths. 
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3.1 The Straight-Line Trajectory Planning Scheme 

The indexer has two main modes of operation: the normal mode and the continuous 
mode. In the normal mode, based on the information about the velocity v f , acceleration 
a and the distance to be traveled A,, the indexer will determine the appropriate leg 
velocity profiles which are either a trapezoid or a triangle depending on the re ationship 
between the given information. For simplicity, the straight-line planning scheme will 
only utilize the trapezoidal profile which is illustrated in Figure 5 where t a , t c , and 
t d denote the acceleration time, the constant velocity time, and the deceleration time 
respectively. In addition the indexer requires that t a = t d . By inspection, we found that 
A, = v f (t c + t d ) and v f = at a . To track a path in a 3-dimensional space, the positions 
of x- y- and z-coordinates must always be linearly related to each other anytime during 
the tracking. Intuitively, if the leg displacements are planned such that their velocities 
are linearly related to each other, then the resulting Cartesian motion should be a 
linear path. Computer simulation which utilized the manipulator forward kinematics 
and was performed to verify the above fact has agreed with our intuition. The following 
algorithm facilitates the trajectory planning for straight lines. 

Algorithm 1: Straight Line Trajectory Planning 

1. Use the manipulator inverse kinematics to compute the leg lengths corresponding 
to the starting point P s and the final point Pj of the straight line, namely l,s an 

Uf for i=l,2,...,6. 

2. Compute An = Uf - l,s for i=l,2,...,6 and find A tk whose absolute value is the 
biggest. 

3. Select a k and v f k for the k-th leg such that a k < a m ax\ and v f k < v m ax; v f k < 
a ks J\A^\ to ensure trapezoidal profile where a m ax and v m ax denote the maximum 

acceleration and velocity of the stepper motor, and then compute t a = = U 

311(1 ~ *“• 

4. For i^k; i=l,2,..,6 compute a, = i o( £+f c j U/,=< ° a ’' 

3.2 The Trajectory Planning Scheme For Arbitrary Curves 

In the continuous mode of operation, the indexer requires the acceleration a, final ve- 
locity v f and the direction of the rotation (direction of the linear displacement) of the 
stepper motor. The stepper motor will accelerate to the velocity v } and continue to run 
at this velocity until new velocity and acceleration are given in the same direction of 
rotation. The current planning mainly consists of dividing a space curve into n segmen s 
and then planning the velocity profiles of the manipulator legs in the continuous mode 
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so that each segment will be reached within a specified time. The following algorithm 
is proposed to plan for arbitrary space curves. 

Algorithm 2: Arbitrary Curve Trajectory Planning 

1. Divide the space curve into n segments. 

2. Use the manipulator inverse kinematics to compute the leg lengths corresponding 
to each segment point on the curve, namely Z.j for i=l,2,...,6 (leg number) and 
j=l,2,...,n+l (segment point number) 

3. Compute A fJ = j + 1 - U,j for i=l,2,...,6 and j=l,2,...n. 

4. For each segment, select an appropriate travel time tj for j=l,2,...,n, and compute 
the corresponding acceleration and final velocity at the end of each segment. 

In general, the travel times for the segments are constant and equal to each other 
during the tracking of curves which do not require the change of leg direction. However 
when direction of any leg has to change, the travel time can be selected efficiently using 
the look ahead method. Using this method, the algorithm looks at the next segment 
point and determine if any change in leg direction is necessary. For example if the 
direction of a leg requires direction change, then its travel time will be recomputed to 
ensure that the velocity at the end of the segment will be zero to allow direction change. 
After that, the recomputed travel time will be set for the remaining legs for the next two 
segments. Finally the travel time of all legs will be set back to the old value before the 
leg direction change occurs. The above process can be repeated any time a leg direction 
change is necessary. 


4 Experimental Study 

This section presents the results of experiments conducted to evaluate the performance 
of the developed trajectory planning schemes. In particular, Algorithm 1 was used 
to track a straight line in the x-y plane of the base frame {B}, which is described 
by y ( t j _ The required profiles of the manipulator legs required to track the 

straight line were determined before the tracking using Algorithm 1 and are illustrated 
in Figure 6. The path that the manipulator gripper actually tracked, is presented in 
Figure 7 together with the desired path. Figure 7 shows excellent tracking capability 
with insignificant errors. Algorithm 2 was applied to compute the leg velocity profiles 
shown in Figure 8 to track a circle in the x-y plane of the {B} with a radius of 0.8 
inches. Figure 9 shows the desired path plotted together with the path the mampulator 
actually tracked. The average errors for x- y- and z-coordinates were computed to be 
1.096xl0 -4 inches, 2.326xl0 -4 inches, and 2.974xl0~ 5 inches, respectively. 
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5 Conclusion 

The problem of trajectory planning for a Stewart Platform-based manipulator was ad- 
dressed in this report. We first described the main components of the manipulator and 
briefly presented the development of its inverse and forward kinematics. We then em- 
ployed the normal and continuous modes of operation of the stepper motor indexer to 
derive two trajectory planning schemes for tracking straight paths and circular paths, 
respectively. Experiments performed to evaluate the developed algorithms showed ex- 
cellent tracking results. Future research will focus on investigating the application of 
the algorithms to carry out assembly tasks such as mating space connectors with or 
without force and position feedback. In addition, development of a 6 DOF force sensor 
system based on the mechanism of the Stewart Platform is under way. 
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Figure 4: Vector diagram for the ith leg. 


Figure 5: Trapezoidal velocity profile. 
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Figure 6: Leg velocity profiles (Alg. 1). 
vertical axis=velocity ( rev/sec ) 


2 
1 
0 

Figure 7: Tracking a straight line. 
vertical axis=y-axis (inch) 
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Figure 8: Velocity profiles for 6 legs (Algorithm 2). 
vertical axis=velocity (revolution/ sec) 



Figure 9: Tracking a circular path. 





